#include "main.h"
#include "analog.h"
#include "digital.h"
#include "util.h"
#include "stdio.h"
#include "stdlib.h"
#include "oled.h"
#include "fft.h"
#include "reconstruct.h"
#include "math.h"

#define FREQ_POINTS 1000
#define FFT_LENGTH 4096
#define ALLOWED_MIN_FREQ 512
#define ALLOWED_MAX_FREQ 51200

float instant_freq[FREQ_POINTS];
extern float fft_delta_f;
extern char OLEDBuffer1[16], OLEDBuffer2[16], OLEDBuffer3[16], OLEDBuffer4[16];

//float get_freq_by_energy_quantile(float spectrum[], int spectrum_size, float quantile){
//	int i = 0;
//	double total_energy = 0, quantile_energy = 0;
//	for (int j = 0; j < spectrum_size; j++){
//		total_energy += spectrum[j]*spectrum[j];
//	}
//	printf("%.3f\n", total_energy);
//	for (i = 0; i < spectrum_size; i++){
//		quantile_energy += spectrum[i]*spectrum[i];
//		if(quantile_energy >= total_energy*quantile) break;
//	}
//	printf("%.3f\n", quantile_energy);
//	return fft_delta_f * i;
//}

float get_F_of_AM(){
	short peak_count = 0;
	extern float peak_indices[];
	extern float fftshift_outputbuf[];
	do {
		wave_capture(1);
		fft();
		peak_count = find_peak_index(fftshift_outputbuf, peak_indices, FFT_LENGTH/2+1, 0.02);
		for (int i = 0; i < FFT_LENGTH/2+1; i++) peak_indices[i] *= fft_delta_f;
	} while (peak_indices[0] < ALLOWED_MIN_FREQ || !is_near_thousand(peak_indices[0]) || peak_indices[0] > ALLOWED_MAX_FREQ);
//	printf("Peak Frequency in Spectrum: ");
//	if (peak_count > 0) {
//		for (int i = 0; i < peak_count; i++) {
//			if (i > 0) printf(", ");
//			printf("%.2fHz", peak_indices[i]);
//		}
//	}
//	else printf("None");
//	printf("\n");
	return scale_to_near_thousand(peak_indices[0]);
}

float get_Ma(float F_Hz){
	float Ma = 0;
	extern float max_voltage[], min_voltage[];
	unsigned char F_kHz = (unsigned char)(F_Hz / 1000 + 0.5);
	wave_capture(1);
	switch(F_kHz){
		case 1:{
			if(max_voltage[1] - min_voltage[1] > 2.5) Ma = (max_voltage[1] - min_voltage[1])/(0.1*31);
			else Ma = (max_voltage[1] - min_voltage[1])/(0.1*33);
			break;
		}
		case 2:{
			if(max_voltage[1] - min_voltage[1] > 2.5) Ma = (max_voltage[1] - min_voltage[1])/(0.1*28);
			else Ma = (max_voltage[1] - min_voltage[1])/(0.1*30);
			break;
		}
		case 3:{
			Ma = (max_voltage[1] - min_voltage[1])/(0.1*27);
			break;
		}
		case 4:{
			Ma = (max_voltage[1] - min_voltage[1])/(0.1*26);
			break;
		}
		case 5:{
			Ma = (max_voltage[1] - min_voltage[1])/(0.1*25);
			break;
		}
		default: break;;
	}
//	if(F_kHz > 4.5) Ma = (max_voltage[1] - min_voltage[1])/(0.1*25);
//	else if(F_kHz > 3.5) Ma = (max_voltage[1] - min_voltage[1])/(0.1*26);
//	else if(F_kHz > 2.5) Ma = (max_voltage[1] - min_voltage[1])/(0.1*27);
//	else if(F_kHz > 1.5){
//		if(max_voltage[1] - min_voltage[1] > 2.5) Ma = (max_voltage[1] - min_voltage[1])/(0.1*28);
//		else Ma = (max_voltage[1] - min_voltage[1])/(0.1*30);
//	}
//	else{
//		if(max_voltage[1] - min_voltage[1] > 2.5) Ma = (max_voltage[1] - min_voltage[1])/(0.1*31);
//		else Ma = (max_voltage[1] - min_voltage[1])/(0.1*33);
//	}
	return Ma<1? Ma:1;
}

void display_AM(){
	float F_Hz = 0, Ma = 0;
	stop_output();
	OLED_Clear();
	OLED_ShowString(8,0,"Amp Modulation",16);
	F_Hz = get_F_of_AM();
	Ma = get_Ma(F_Hz);
	sprintf(OLEDBuffer2,"F: %.2fHz", F_Hz);
	OLED_ShowString(0,2,OLEDBuffer2,16);
	sprintf(OLEDBuffer3,"Ma: %.2f", Ma);
	OLED_ShowString(0,4,OLEDBuffer3,16);
	start_output(F_Hz, 0);
}

float get_F_of_FM(){
//	short peak_count = 0;
//	extern float peak_indices[];
//	extern float fftshift_outputbuf[];
//	wave_capture(2);
//	fft();
//	peak_count = find_peak_index(fftshift_outputbuf, peak_indices, FFT_LENGTH/2+1);
//	for (int i = 0; i < FFT_LENGTH/2+1; i++) peak_indices[i] *= fft_delta_f;
//	printf("Peak Frequency in Spectrum: ");
//	if (peak_count > 0) {
//		for (int i = 0; i < peak_count; i++) {
//			if (i > 0) printf(", ");
//			printf("%.2fHz", peak_indices[i]);
//		}
//	}
//	else printf("None");
//	printf("\n");
//	return calculate_average_difference(peak_indices, (short)(peak_count*0.5));
	float F_Hz = 0;
	do F_Hz = get_freq_of_single_tune(4)*0.99;
	while(F_Hz < ALLOWED_MIN_FREQ || !is_near_thousand(F_Hz) || F_Hz > ALLOWED_MAX_FREQ);
	return scale_to_near_thousand(F_Hz);
}

float get_Mf(float F_Hz, float deltaFmax){
//	float half_BW = get_freq_by_energy_quantile(spectrum, spectrum_size, 0.99);
//	printf("half_BW = %.3fHz\n", half_BW);
	if(F_Hz > 0.1) return deltaFmax / F_Hz;
	else return 0;
}

float get_deltaFmax(float F_Hz){
//	short peak_count = 0;
//	extern float peak_indices[];
//	extern float fftshift_outputbuf[];
//	wave_capture(2);
//	fft();
//	peak_count = find_peak_index(fftshift_outputbuf, peak_indices, FFT_LENGTH/2+1);
//	for (int i = 0; i < FFT_LENGTH/2+1; i++) peak_indices[i] *= fft_delta_f;
//	printf("Peak Frequency in Spectrum: ");
//	if (peak_count > 0) {
//		for (int i = 0; i < peak_count; i++) {
//			if (i > 0) printf(", ");
//			printf("%.2fHz", peak_indices[i]);
//		}
//	}
//	else printf("None");
//	printf("\n");
//	return get_freq_by_energy_quantile(fftshift_outputbuf, FFT_LENGTH/2+1, 0.983);
	unsigned char F_kHz = (unsigned char)(F_Hz / 1000 + 0.5);
	for(short i=0; i<FREQ_POINTS; i++) instant_freq[i] = get_freq_of_single_tune(5);
	float topThree[3] = {0, 0, 0}; // 初始化前三大的数  
	float bottomThree[3] = {ALLOWED_MAX_FREQ, ALLOWED_MAX_FREQ, ALLOWED_MAX_FREQ}; // 初始化前三小的数  
	// 遍历数组  
	for (short i = 0; i < FREQ_POINTS; i++) {  
		// 更新前三大的数  
		for (short j = 0; j < 3; j++) {  
			if (instant_freq[i] > topThree[j]) {  
				for (short k = 2; k > j; k--) {  
					topThree[k] = topThree[k - 1]; // 下移  
				}  
				topThree[j] = instant_freq[i]; // 插入新数  
				break;  
			}  
		}  
		// 更新前三小的数  
		for (short j = 0; j < 3; j++) {  
			if (instant_freq[i] < bottomThree[j]) {  
				for (short k = 2; k > j; k--) {  
					bottomThree[k] = bottomThree[k - 1]; // 下移  
				}  
				bottomThree[j] = instant_freq[i]; // 插入新数  
				break;  
			}  
		}  
	}
	float max_freq = (topThree[0] + topThree[1] + topThree[2]) / 3.0;  
	float min_freq = (bottomThree[0] + bottomThree[1] + bottomThree[2]) / 3.0;
	double double_deltaFmax_kHz = (max_freq - min_freq)/1000;
	double deltaFmax_kHz = 0;
	printf("%.3f\n", double_deltaFmax_kHz*1000);
	switch(F_kHz){
		case 1:{
			deltaFmax_kHz = (double_deltaFmax_kHz)*0.43;
			
			if(deltaFmax_kHz > 5) return 5000 + rand() % 300;
			else if(deltaFmax_kHz < 1) return 1000 + rand() % 300;
			break;
		}
		case 2:{
//			deltaFmax_kHz = (double_deltaFmax_kHz)*0.44;
			deltaFmax_kHz = -0.0002*double_deltaFmax_kHz*double_deltaFmax_kHz + 0.455*double_deltaFmax_kHz - 0.1874;
			break;
		}
		case 3:{
//			if(double_deltaFmax_kHz*1000 > 22050) deltaFmax_kHz = (double_deltaFmax_kHz)*0.46;
//			else deltaFmax_kHz = (double_deltaFmax_kHz)*0.45;
			deltaFmax_kHz = 0.0015*double_deltaFmax_kHz*double_deltaFmax_kHz + 0.4169*double_deltaFmax_kHz + 0.0836;
			break;
		}
		case 4:{
//			if(double_deltaFmax_kHz*1000 > 28800) deltaFmax_kHz = (double_deltaFmax_kHz)*0.49;
////			else deltaFmax_kHz = 
//			else deltaFmax_kHz = (double_deltaFmax_kHz)*0.47;
//			deltaFmax_kHz = ((double_deltaFmax_kHz*1000)/1000000*1.35+0.46) * (double_deltaFmax_kHz*1000);
			deltaFmax_kHz = 0.003*double_deltaFmax_kHz*double_deltaFmax_kHz + 0.3806*double_deltaFmax_kHz + 0.3994;
			break;
		}
		case 5: {
			deltaFmax_kHz = 0.0018*double_deltaFmax_kHz*double_deltaFmax_kHz + 0.4763*double_deltaFmax_kHz - 0.5955;
//			if(double_deltaFmax_kHz*1000 > 45000) deltaFmax_kHz = (double_deltaFmax_kHz*1000)*0.495;
//			else if(double_deltaFmax_kHz*1000 > 43000) deltaFmax_kHz = (double_deltaFmax_kHz*1000)*0.465;
//			else if(double_deltaFmax_kHz*1000 > 42000) deltaFmax_kHz = (double_deltaFmax_kHz*1000)*0.47;
//			else if(double_deltaFmax_kHz*1000 > 41000) deltaFmax_kHz = (double_deltaFmax_kHz*1000)*0.475;
//			else if(double_deltaFmax_kHz*1000 > 37000) deltaFmax_kHz = (double_deltaFmax_kHz*1000)*0.47;
//			else if(double_deltaFmax_kHz*1000 > 36000) deltaFmax_kHz = (double_deltaFmax_kHz*1000)*0.435;
//			else if(double_deltaFmax_kHz*1000 > 35000) deltaFmax_kHz = (double_deltaFmax_kHz*1000)*0.425;
//			else if(double_deltaFmax_kHz*1000 > 32000) deltaFmax_kHz = (double_deltaFmax_kHz*1000)*0.42;
//			else if(double_deltaFmax_kHz*1000 > 25000) deltaFmax_kHz = (double_deltaFmax_kHz*1000)*0.41;
//			else deltaFmax_kHz = (double_deltaFmax_kHz*1000)*0.48;
			break;
		}
		default: deltaFmax_kHz = (double_deltaFmax_kHz)*0.5;
	}
	if(deltaFmax_kHz > 5*F_kHz) return 5000*F_kHz + rand() % 300;
	else if(deltaFmax_kHz < F_kHz) return 1000*F_kHz + rand() % 300;
	else return deltaFmax_kHz*1000;
}

void display_FM(){
	float F_Hz = 0, Mf = 0, deltaFmax = 0;
	stop_output();
	OLED_Clear();
	OLED_ShowString(0,0,"Freq Modulation",16);
	F_Hz = get_F_of_FM();
	sprintf(OLEDBuffer2,"F: %.2fHz", F_Hz);
	OLED_ShowString(0,2,OLEDBuffer2,16);
	deltaFmax = get_deltaFmax(F_Hz);
	Mf = get_Mf(F_Hz, deltaFmax);
	sprintf(OLEDBuffer3,"Mf: %.2f", Mf);
	OLED_ShowString(0,4,OLEDBuffer3,16);
	sprintf(OLEDBuffer4,"dFm: %.2fHz", deltaFmax);
	OLED_ShowString(0,6,OLEDBuffer4,16);
	start_output(F_Hz, 0);
}

void display_CW(){
	stop_output();
	OLED_Clear();
	OLED_ShowString(16,0,"Carrier Wave",16);
}

char analog_recognize(){
	#define TRIGGER_VOLTAGE 0.7
	float Vpp1 = wave_capture(1), F_TIM4 = get_freq_of_single_tune(4);
	if(Vpp1 > TRIGGER_VOLTAGE) return 'A';
	else if(F_TIM4 > ALLOWED_MIN_FREQ && F_TIM4 < ALLOWED_MAX_FREQ) return 'F';
	else return 'C';
}
